clear
clc

target_point=[5,5];
obs_point=[2,2];
car_point=[0,0];
b=0.2;

oio_cos=dot([1,0],obs_point)/norm(obs_point);
oio=acos(oio_cos);
if(obs_point(2)<0)
    oio=-oio;
end

tio_cos=dot([1,0],target_point)/norm(target_point);
tio=acos(tio_cos);
if(target_point(2)<0)
    tio=-tio;
end

car_angle=0;

dg=norm(target_point-car_point);
do=norm(obs_point-car_point);
car_dir=[1*cos(car_angle),1*sin(car_angle)];
% sig=acos((target_point(1)-car_point(1))/dg);
% if(target_point(2)-car_point(2)<0)
%     sig=-sig;
% end
% sio=acos((obs_point(1)-car_point(1))/do);
% if(obs_point(2)-car_point(2)<0)
%     sio=-sio;
% end
sio_dot=dot(car_dir,obs_point-car_point);
sio_cos=sio_dot/(do);
sio=acos(sio_cos);
if(car_angle>=oio)
    sio=-sio;
end

sig_dot=dot(car_dir,target_point-car_point);
sig_cos=sig_dot/(dg);
sig=acos(sig_cos);
if(car_angle>=tio)
    sig=-sig;
end

time_step=0.05;

v=0;
w=0;
plot(target_point(1),target_point(2),'go');
hold on
%plot(obs_point(1),obs_point(2),'bo');
vg=[];
vLg=[];
vRg=[];
wg=[];
for i=1:20000
%     mov_dis=v*time_step;
%     mov_ang=w*time_step;
%     car_angle=car_angle+mov_ang;
%     car_point(1)=car_point(1)+mov_dis*cos(car_angle);
%     car_point(2)=car_point(2)+mov_dis*sin(car_angle);
    P=[car_point(1);car_point(2);car_angle];
    q=[v;w];
    P=P+time_step*[cos(car_angle),0;sin(car_angle),0;0,1]*q;
    car_point=[P(1),P(2)];
    car_angle=P(3);
    plot(car_point(1),car_point(2),'r*');
    hold on
    car_dir=[1*cos(car_angle),1*sin(car_angle)];
    %
    dg=norm(target_point-car_point);
%     do=norm(obs_point-car_point);
%     if(do>4)
%         do=4;
%     end
    %
    sig_dot=dot(car_dir,target_point-car_point);
    sig_cos=sig_dot/(dg);
    sig=acos(sig_cos);
    if(car_angle>=tio)
        sig=-sig;
    end
    if(sig>1.4)
        sig=1.4;
    elseif(sig<-1.4)
        sig=-1.4;
    end
    %
%     sio_dot=dot(car_dir,obs_point-car_point);
%     sio_cos=sio_dot/(do);
%     sio=acos(sio_cos);
%     if(car_angle>=oio)
%         sio=-sio;
%     end
%     if(sio>1.4)
%         sio=1.4;
%     end
%     if(sio<-1.4)
%         sio=-1.4;
%     end
    %
    if(mod(i,500)==0||i==1)
        do_max=randi([2,5]);
        sio_max=randi([-60,60]);
    end
    do=do_max-rand(1);
    sio=randi([sio_max-10,sio_max]);
    sio=sio/360*2*pi;
    do=5;
    sio=-pi/2;
    v_o=v;
    w_o=w;
    obx=cos(car_angle)+rand(1)/10;
    oby=sin(car_angle)+rand(1)/10;
    if(dg<0.3)
        [vL,vR]=[0,0];
    else
        [vL,vR]=RFNN_EKF_cal(dg,do,sig,sio,v_o,w_o);
    end
%     v=(vL+vR)/2;
%     w=(vR-vL)/(2*b);
    v=vR;
    w=vL;
    vg=[vg,v];
    vLg=[vLg,vL];
    vRg=[vRg,vR];
    wg=[wg,w];
end